#include <pid_lib/pid.h>
#include "stdlib.h"

//#include "arm_math.h"
#include "math.h"

#define RESET_VAL 0.0f

void pid_init (pid_dat_t* pid)
{
 pid->err = RESET_VAL;
 pid->last_err = RESET_VAL;
 pid->prev_last_err = RESET_VAL;

 pid->output = RESET_VAL;
 pid->integral_sum = RESET_VAL;
}
void pid_set_kp (pid_dat_t* pid,float kp)
{
 pid->kp = kp;
}
void pid_set_ki (pid_dat_t* pid,float ki)
{
 pid->ki = ki;
}
void pid_set_kd (pid_dat_t* pid,float kd)
{
 pid->kd = kd;
}

void pid_reset_integral (pid_dat_t* pid)
{
 pid->integral_sum = RESET_VAL;
}

void pid_set_intergral_limit (pid_dat_t* pid,float integral_limit)
{
 pid->integral_limit = integral_limit;
}
void pid_set_tag (pid_dat_t* pid,float tag)
{
 pid->tag = tag;
}

void pid_set_output_limit (pid_dat_t* pid,float output_limit)
{
 pid->output_limit = output_limit;
}

float pid_calc(pid_dat_t *p_pid_dat, float cur_val)
{
  p_pid_dat->cur = cur_val;

  p_pid_dat->err = p_pid_dat->cur - p_pid_dat->tag;

  p_pid_dat->integral_sum += p_pid_dat->err;
  if(p_pid_dat->integral_sum > p_pid_dat->integral_limit) {p_pid_dat->integral_sum = p_pid_dat->integral_limit;}
  if(p_pid_dat->integral_sum < (p_pid_dat->integral_limit*(-1.0f)) ) {p_pid_dat->integral_sum = (p_pid_dat->integral_limit*(-1.0f));}

  #if 1 //0:inc_pid 1:pos_pid
  //---position PID
  p_pid_dat->P_OUT = p_pid_dat->kp * p_pid_dat->err;
  p_pid_dat->I_OUT = p_pid_dat->ki * p_pid_dat->integral_sum;
  p_pid_dat->D_OUT = p_pid_dat->kd * (p_pid_dat->err - p_pid_dat->last_err);
  p_pid_dat->output = p_pid_dat->P_OUT + p_pid_dat->I_OUT + p_pid_dat->D_OUT;
  #else
  //---increase PID
  p_pid_dat->P_OUT = p_pid_dat->kp * (p_pid_dat->err - p_pid_dat->last_err);
  p_pid_dat->I_OUT = p_pid_dat->ki * p_pid_dat->err;
  p_pid_dat->D_OUT = p_pid_dat->kd * (p_pid_dat->err + p_pid_dat->prev_last_err - 2*p_pid_dat->last_err);
  p_pid_dat->output += p_pid_dat->P_OUT + p_pid_dat->I_OUT + p_pid_dat->D_OUT;
  #endif

  p_pid_dat->prev_last_err = p_pid_dat->last_err;
  p_pid_dat->last_err = p_pid_dat->err;

  if(p_pid_dat->output > p_pid_dat->output_limit) {p_pid_dat->output = p_pid_dat->output_limit;}
  if(p_pid_dat->output < -p_pid_dat->output_limit) {p_pid_dat->output = -p_pid_dat->output_limit;}

  return p_pid_dat->output;
}

float pid_get_err(pid_dat_t *p_pid_dat)
{
 return p_pid_dat->err;
}
